#include "StdAfx.h"
#include "LineTracker.h"

#include <opencv\cv.h>
#include <vector>

using namespace std;
using namespace cv;
//Currently NOT USED. Might help to improve speed.


//Need to track intersection pts!! Not line ending pts. Line ending pts at inf.
LineTracker::LineTracker(Mat origImg, Line startLine) : _startLine(startLine)
{
	//origImg.copyTo(_startImg);
	cvtColor(origImg, _startImg, CV_BGR2GRAY);

	vector<Point> pts = startLine.getPoints();
	_p1 = pts[0];
	_p2 = pts[1];

	Size winSize(40,40);
	windowSize = winSize;

	Rect p1Win = Rect(_p1.x - 10, _p1.y - 10, 20, 20);
	cout << "P1 win x: " << p1Win.x << " y: " << p1Win.y << endl;
	
	_tempP1 = Mat(_startImg, p1Win);

	Rect p2Win = Rect(_p2.x - 10, _p2.y - 10, 20, 20);
	_tempP2 = Mat(_startImg, p2Win);
}

void LineTracker::update(Mat newImg) {

	Mat grayNew;
	cvtColor(newImg, grayNew, CV_BGR2GRAY);

	//Construct window bounds for P1
	Rect p1WindowBounds = Rect(_p1.x-20,_p1.y-20,40,40); 
	Mat p1SearchWindow(grayNew, p1WindowBounds);
	Point newP1Loc = templateMatch(p1SearchWindow, _tempP1);
	newP1Loc.x = newP1Loc.x + _p1.x - 20;
	newP1Loc.y = newP1Loc.y + _p1.y - 20;
	_p1.x = newP1Loc.x;
	_p1.y = newP1Loc.y;

	rectangle(newImg, newP1Loc, Point(newP1Loc.x + 20, newP1Loc.y + 20), Scalar(255,0,0),5);

	//Construct window bounds for P2
	Rect p2WindowBounds = Rect(_p2.x-20, _p2.y-20, 40, 40); 
	Mat p2SearchWindow(grayNew, p2WindowBounds);
	Point newP2Loc = templateMatch(p2SearchWindow, _tempP2);
	newP2Loc.x = newP2Loc.x + _p2.x - 20;
	newP2Loc.y = newP2Loc.y + _p2.y - 20;
	_p2.x = newP2Loc.x;
	_p2.y = newP2Loc.y;

	rectangle(newImg, newP2Loc, Point(newP2Loc.x + 20, newP2Loc.y + 20), Scalar(255,0,0),5);
}

Point LineTracker::templateMatch(Mat searchWindow, Mat templ) {
	Mat result(searchWindow.rows - templ.rows + 1, searchWindow.cols - templ.cols + 1, CV_32FC1);
	matchTemplate(searchWindow, templ, result, CV_TM_SQDIFF);
	normalize(result, result, 0, 1, NORM_MINMAX, -1, Mat());
	Point match;
	Point minLoc, maxLoc;
	double minVal, maxVal;

	minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, Mat());

	return minLoc;
}


LineTracker::~LineTracker(void)
{
}
